# -*- coding: utf-8 -*-

from elp import *
# from matplotlib import pyplot as plt
import time

SetEscape([3.0, -3.0])
SetPurserList([
    Purser(-0.5, -1.0),
    Purser(3.5, 1.0),
    Purser(0.0, 1.0)
    # Purser(4.429345087989051, -1.7069768000690235),
])


opt = casadi.Opti()
elpZ = Ellipse(opt)
opt.minimize(elpZ.a**(2)+elpZ.b**(2))
for i in range(Z.shape[1]):
    opt.subject_to(elpZ(Z[0, i], Z[1, i]) <= -0.1)
elpZ.solve(opt)

b.Emit("plot", [id(Z), Z[0].tolist(), Z[1].tolist(), 0x00ff00])
b.Emit("plot", [id(elpZ), *tl(elpZ.sample()), 0x00ff00])
# b.Emit("scatter", [id(g_E), g_E[0], g_E[1], 0xff0000])
# b.wait()

while True:
    for p in GetPurserList():
        if p.elp is None:
            p.newTarget(elpZ)
    for p in GetPurserList():
        p.move()
    time.sleep(0.1)